#include "filter.h"


#define N 40   //均值滤波

#define M 3    //滑动滤波窗口值



float averageFilter(float *intput)
{
    uint16_t sum = 0;
    uint8_t i;
    for(i = 0; i < N; ++i)
    {
        sum +=*intput  ;
    }
	*intput = sum / N;
//    return sum/N;
}

void Array_Filter(int (*inputArray)[6], int *outputArray)
{
    // 遍历数组的每一列
    for (int j = 0; j < 6; j++)
    {
        int sum = 0;
        // 计算当前列的均值
        for (int i = 0; i < 10; i++)
        {
            sum += inputArray[i][j];
        }
        outputArray[j] = sum / 10; // 计算均值并存储到新的数组中
    }
}





#define BUFFER_SIZE 10 // 缓冲区数组行数
#define COLUMN_SIZE 6 // 缓冲区数组列数


// 初始化滑动均值滤波器
void MovingAverageFilter_Init(MovingAverageFilter_t *filter, int *buffer, uint16_t size)
{
    filter->buffer = buffer;
    filter->size = size;
    filter->pos = 0;
    filter->sum = 0;
    for (uint16_t i = 0; i < size; i++) {
        buffer[i] = 0;
    }
}

// 更新滤波器数据
void MovingAverageFilter_Update(MovingAverageFilter_t *filter, int data)
{
    filter->sum -= filter->buffer[filter->pos];
    filter->sum += data;
    filter->buffer[filter->pos] = data;
    filter->pos++;
    if (filter->pos >= filter->size) {
        filter->pos = 0;
    }
}

// 计算滤波器输出
float MovingAverageFilter_ComputeOutput(MovingAverageFilter_t *filter)
{
    return filter->sum / filter->size;
}

// 滑动均值滤波函数
void MovingAverageFilter_Array(int input[][COLUMN_SIZE], int *output)
{
    MovingAverageFilter_t filters[COLUMN_SIZE];
    int  buffer[COLUMN_SIZE][BUFFER_SIZE];

    // 初始化滤波器
    for (uint16_t i = 0; i < COLUMN_SIZE; i++) {
        MovingAverageFilter_Init(&filters[i], buffer[i], BUFFER_SIZE);
    }

    // 循环计算滤波器输出
    for (uint16_t j = 0; j < COLUMN_SIZE; j++) {
        for (uint16_t i = 0; i < BUFFER_SIZE; i++) {
            MovingAverageFilter_Update(&filters[j], input[i][j]);
        }
        output[j] = MovingAverageFilter_ComputeOutput(&filters[j]);
    }
}

void Limit_float(float *test, float max, float min)//限幅函数
{
	if(*test >= max)
	{
		*test = max;
	}
	if(*test <= min)
	{
		*test = min;
	}
}

void Limit_uint32(uint32_t *test, float max, float min)//限幅函数
{
	if(*test >= max)
	{
		*test = max;
	}
	if(*test <= min)
	{
		*test = min;
	}
}

float kalmanFilter(KFP* kfp, float input)
{
    //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    kfp->Now_P = kfp->LastP + kfp->Q;
    //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
    //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kfp->out = kfp->out + kfp->Kg * (input - kfp->out);//因为这一次的预测值就是上一次的输出值
    //更新协方差方程: 本次的系统协方差付给 kfp->LastP 为下一次运算准备。
    kfp->LastP = (1 - kfp->Kg) * kfp->Now_P;
    return kfp->out;
}
